
#include "cmd_func.h"
#include "stdlib.h"
#include "../uart/oslib_uart.h"
#include "FreeRTOS.h"
#include "task.h"

#include "dynamixel2.h"
#include "servo.h"
#include "Dog.h"
#include "utils.h"


#ifdef OSLIB_CAN_MODULE_ENABLED
    #include "../can/oslib_can.h"
#endif

#if defined(OSLIB_UART_MODULE_ENABLED) && USE_OSLIB_UART_CLI
/* 在这里添加命令回调函数的定义或外部声明 */
static void Command_Hello(int argc, char *argv[])
{
    uprintf("Hello SimpleLib!\r\n");
}

static void Command_Echo(int argc, char *argv[])
{
    for (int i = 1; i < argc; i++)
        uprintf("Echo: %s\r\n", argv[i]);
}

#ifdef OSLIB_CAN_MODULE_ENABLED
static void Command_CanSend(int argc, char *argv[])
{
    if (argc >= 3) {
        can_msg msg;
        msg.in[0] = 0x40005850;
        msg.in[1] = 0x00000000;
        uint32_t id = strtol(argv[2], NULL, 16);
        if (argv[1][0] == 's') {
            can_send_msg(id, &msg);
            uprintf("CanSend:std[%x]\r\n", id);
        } else if (argv[1][0] == 'e') {
            can_ext_send_msg(id, &msg);
            uprintf("CanSend:ext[%x]\r\n", id);
        } else
            uprintf("Param Error!\r\n");
    } else {
        uprintf("Param Error!\r\n");
    }
}
#endif

static void Command_Task(int argc, char *argv[])
{
    uprintf("Free Heap: %dB\r\n", xPortGetFreeHeapSize());
#if ((configUSE_TRACE_FACILITY == 1) && (configUSE_STATS_FORMATTING_FUNCTIONS > 0) && (configSUPPORT_DYNAMIC_ALLOCATION == 1))
    static char tasklist_buffer[256];
    uprintf("Name          \tState\tPrio\tStack\tRank\r\n");
    vTaskList(tasklist_buffer);
    uprintf(tasklist_buffer);
#endif
    // uprintf("Name          \tCount\tUtilization\r\n");
    // vTaskGetRunTimeStats(tasklist_buffer);
    // uprintf(tasklist_buffer);
}


static void Command_nijie(int argc, char *argv[])
{
    double a[3] = {0};

    a[0] = (double)atof(argv[1]);
    a[1] = (double)atof(argv[2]);
    a[2] = (double)atof(argv[3]);
    simulate(a);
    uprintf("\nnijie done \n\r");
}
static void Command_ShowBroadcast(int argc, char *argv[])
{
    show_broadcast();
}
static void Command_inverse(int argc, char *argv[])
{
    double a[3] = {0};

    a[0] = (double)atof(argv[1]);
    a[1] = (double)atof(argv[2]);
    a[2] = (double)atof(argv[3]);
    // cal(a);
    uprintf("inverse done \n\r");
}

static void Command_Half(int argc, char *argv[])
{
    if (strcmp(argv[1], "led") == 0)
    {
        Servo_Led(1, 1);
    }
    else if (strcmp(argv[1], "ang") == 0)
    {
        // uint8_t id = (uint8_t)(atoi(argv[2]));
        // float angle = atof(argv[3]);
        // uint32_t pos = Servo_Angle2Pos_Mappping(angle, id);
        // Servo_Goal_Position(id, pos);
        // uprintf("id %d ang %.2f pos %d\n\r", id, angle, pos);
    }
    else if (strcmp(argv[1], "leg_reset") == 0)
    {
        uint8_t id = (uint8_t)(atoi(argv[2]));
        Servo_Sync_ResetLeg(id);
        // if (id == 99)
        // {
        //     Servo_Torque(0xFE, 1);
        //     Servo_Goal_Position(0xFE, 2048);
        // }
        // else
        // {
        //     Servo_Torque(id * 10, 1);
        //     Servo_Torque(id * 10 + 1, 1);
        //     Servo_Torque(id * 10 + 2, 1);
        //     Servo_Goal_Position(id * 10, 2048);
        //     Servo_Goal_Position(id * 10 + 1, 2048);
        //     Servo_Goal_Position(id * 10 + 2, 2048);
        // }
    }
    else if (strcmp(argv[1], "tor") == 0)
    {
        int id = (atoi(argv[2]));
        bool TrqState = (bool)(atoi(argv[3]));
        if (id == -1)
        {
            Servo_Sync_Torque(TrqState);
        }
        else
        {
            Servo_Torque((uint8_t)id, TrqState);
            uprintf("id %d tor %d\n\r", id, TrqState);
        }
    }
    else if (strcmp(argv[1], "pos") == 0)
    {
        uint8_t id = (uint8_t)(atoi(argv[2]));
        uint32_t pos = (uint32_t)(atoi(argv[3]));
        Servo_Goal_Position(id, pos);
        uprintf("id %d pos %d\n\r", id, pos);
    }
    else if (strcmp(argv[1], "init") == 0)
    {
        Servo_Sync_Torque(1);
        Dog_Gait_Init();
        Servo_Sync_Goal_Position(broadcast_ctrl_pos);
        uprintf("dog gait init \n\r");
    }
    else if (strcmp(argv[1], "sync_pos") == 0)
    {
        float ang[12] = {0};
        ang[0] = atof(argv[2]);
        ang[1] = atof(argv[3]);
        ang[2] = atof(argv[4]);
        ang[3] = atof(argv[5]);
        ang[4] = atof(argv[6]);
        ang[5] = atof(argv[7]);
        ang[6] = atof(argv[8]);
        ang[7] = atof(argv[9]);
        ang[8] = atof(argv[10]);
        ang[9] = atof(argv[11]);
        ang[10] = atof(argv[12]);
        ang[11] = atof(argv[13]);

        uint32_t pos[12] = {2048};
        pos[0] = (uint32_t)(-11.36 * ang[0] + 2048);
        pos[1] = (uint32_t)(11.38 * ang[1] + 2048);
        pos[2] = (uint32_t)(11.38 * ang[2] + 2048);
        pos[3] = (uint32_t)(11.36 * ang[3] + 2048);
        pos[4] = (uint32_t)(-11.38 * ang[4] + 2048);
        pos[5] = (uint32_t)(-11.38 * ang[5] + 2048);
        pos[6] = (uint32_t)(-11.36 * ang[6] + 2048);
        pos[7] = (uint32_t)(-11.38 * ang[7] + 2048);
        pos[8] = (uint32_t)(-11.38 * ang[8] + 2048);
        pos[9] = (uint32_t)(11.36 * ang[9] + 2048);
        pos[10] = (uint32_t)(11.38 * ang[10] + 2048);
        pos[11] = (uint32_t)(11.38 * ang[11] + 2048);

        // for (int i = 0; i < 4; i++)
        // {
        //     for (int j = 0; j < 3; j++)
        //     {
        //         pos[i * 3 + j] = Servo_Angle2Pos_Mappping(ang[i * 3 + j], i * 10 + j);
        //     }
        // }

        Servo_Sync_Goal_Position(pos);
        uprintf("sync_pos\n\r");
    }
    else if (strcmp(argv[1], "run") == 0)
    {
        Servo_Sync_Torque(1);
        Dog.ctrl_mode = DOG_MODE_RUN;
        uprintf("DOG_MODE_RUN\n\r");
    }
    else if (strcmp(argv[1], "leg_pos") == 0)
    {
        uint8_t id = (uint8_t)(atoi(argv[2]));
        float end_x = atof(argv[3]);
        float end_y = atof(argv[4]);
        float end_z = atof(argv[5]);
        // uprintf("%f %f %f \n\r", atof(argv[3]), atof(argv[4]), atof(argv[5]));
        Dog.legs[id].foot_x = end_x;
        Dog.legs[id].foot_y = end_y;
        Dog.legs[id].foot_z = end_z;
        uprintf("foot:(%f %f %f)\n\r", end_x, end_y, end_z);
        Dog_Inverse_kinematics(&Dog.legs[id]);
        Dog_servo_angle_pos();
        Dog.ctrl_mode = DOG_MODE_DEBUG;
        uprintf("inverse reslt %f %f %f\n\r", Dog.legs[id].servo[0].goal_angle, Dog.legs[id].servo[1].goal_angle, Dog.legs[id].servo[2].goal_angle);
    }
    else if (strcmp(argv[1], "adjust") == 0)
    {
        delta_h = atof(argv[2]);
        delta_s = atof(argv[3]);
        delta_t = atof(argv[4]);
        delta_ds = atof(argv[5]);
        delta_dth = atof(argv[6]);
        uprintf("delta_h=%f, delta_s=%f, delta_t=%f, delta_ds=%f, delta_dth=%f\n\r", delta_h, delta_s, delta_t, delta_ds, delta_dth);
    }

    else if (strcmp(argv[1], "none") == 0)
    {
        Dog.ctrl_mode = DOG_MODE_NONE;
    }
}

static void Command_Dog(int argc, char *argv[])
{
    if (strcmp(argv[1], "mode") == 0)
    {
        Dog.ctrl_mode = atoi(argv[2]);
        if (Dog.ctrl_mode == 13 || Dog.ctrl_mode == 11)
        {
            reset_time(&Dog_time);
            Dog.upstair_flag = 0;
        }
        uprintf("switch dog mode to %d", Dog.ctrl_mode);
    }
    if (strcmp(argv[1], "show") == 0)
    {
        uprintf("delta_t\t=    %f\n", delta_t);
        uprintf("delta_h\t=    %f\n", delta_h);
        uprintf("delta_s\t=    %f\n", delta_s);
        uprintf("delta_ds\t=    %f\n", delta_ds);
        uprintf("delta_dth\t=    %f\n", delta_dth);
        uprintf("delta_Dth\t=    %f\n", delta_Dth);
        uprintf("delta_dz\t=    %f\n", delta_dz);
        uprintf("dz_k\t=    %f\n", dz_k);
        uprintf("delta_dh\t=    %f\n", delta_dh);
        uprintf("barycenter_front\t=    %f\n", barycenter_front);
        uprintf("barycenter_back\t=    %f\n", barycenter_back);
        uprintf("barycenter_left\t=    %f\n", barycenter_left);
        uprintf("barycenter_right\t=    %f\n", barycenter_right);
        uprintf("\n\r");
    }
    else if (strcmp(argv[1], "adjust") == 0)
    {
        delta_t = atof(argv[2]);
        uprintf("delta_t\t=    %f\n", delta_t);
        delta_h = atof(argv[3]);
        uprintf("delta_h\t=    %f\n", delta_h);
        delta_s = atof(argv[4]);
        uprintf("delta_s\t=    %f\n", delta_s);
        delta_ds = atof(argv[5]);
        uprintf("delta_ds\t=    %f\n", delta_ds);
        delta_dth = atof(argv[6]);
        uprintf("delta_dth\t=    %f\n", delta_dth);
        delta_Dth = atof(argv[7]);
        uprintf("delta_Dth\t=    %f\n", delta_Dth);
        delta_dz = atof(argv[8]);
        uprintf("delta_dz\t=    %f\n", delta_dz);
        dz_k = atof(argv[9]);
        uprintf("dz_k\t=    %f\n", dz_k);
        delta_dh = atof(argv[10]);
        uprintf("delta_dh\t=    %f\n", delta_dh);
        uprintf("\n\r");
    }
    else if (strcmp(argv[1], "center") == 0)
    {

        barycenter_front = atof(argv[2]);
        uprintf("barycenter_front\t=    %f\n", barycenter_front);
        barycenter_back = atof(argv[3]);
        uprintf("barycenter_back\t=    %f\n", barycenter_back);
        barycenter_left = atof(argv[4]);
        uprintf("barycenter_left\t=    %f\n", barycenter_left);
        barycenter_right = atof(argv[5]);
        uprintf("barycenter_right\t=    %f\n", barycenter_right);
        barycenter_central = atof(argv[6]);
        uprintf("barycenter_central\t=    %f\n", barycenter_central);
        uprintf("\n\r");
    }
}

/* 在这里完成命令到回调函数的映射 */
void Command_Init()
{
    OSLIB_UartCmd_AddCommand("hello", "\"Hello SimpleLib!\"", Command_Hello);   /* 输出 "Hello SimpleLib!" */   // 测试串口输出和命令
    OSLIB_UartCmd_AddCommand("echo", "Echo message", Command_Echo);             /* 回显参数的内容 */              // 测试命令参数解析
#ifdef OSLIB_CAN_MODULE_ENABLED
    OSLIB_UartCmd_AddCommand("cansend", "CAN send message", Command_CanSend);   /* 发送CAN报文, 内容为两个整数 */  // 测试CAN发送
#endif
    OSLIB_UartCmd_AddCommand("tasks", "Show task list", Command_Task);
    OSLIB_UartCmd_AddCommand("half", "", Command_Half);
    OSLIB_UartCmd_AddCommand("nijie", "", Command_nijie);
    OSLIB_UartCmd_AddCommand("inverse", "", Command_inverse);
    OSLIB_UartCmd_AddCommand("show", "", Command_ShowBroadcast);

    OSLIB_UartCmd_AddCommand("dog", "mode adjust[9]", Command_Dog);

}

#endif
